from math import *;
import numpy;
		
		
def vector(x1,x2,x3):
	return numpy.array([[x1],[x2],[x3]]);
		
def RotateX(x,th):
	Rx=numpy.array([[1.0,0.0,0.0],[0.0,cos(th),-sin(th)],[0.0,sin(th),cos(th)]]);
	return Rx.dot(x);

def RotateY(x,th):
	Ry=numpy.array([[cos(th),1.0,-sin(th)],[0.0,1.0,0.0],[sin(th),cos(th),0.0]]);
	return Ry.dot(x);
	
def RotateZ(x,th):
	Rx=numpy.array([[cos(th),-sin(th),0.0],[sin(th),cos(th),0.0],[0.0,0.0,1.0]]);
	return Rz.dot(x);

def innerp(x,y):
	s=0.0;
	for i in range(len(x)):
		s+=x[i][0]*y[i][0];
	return s;


# ====================================================		
# COORDINATE SYSTEM
# ====================================================		
class CoordSys:
	def __init__(self,c,Th):
		self.thx=Th[0];
		self.thy=Th[1];
		self.thz=Th[2];
		self.c=numpy.array([[c[0]],[c[1]],[c[2]]]);
		self.Th=numpy.array([[Th[0]],[Th[1]],[Th[2]]]);

	def getC(self):
		return [self.c[0][0],self.c[1][0],self.c[2][0]];

	def getTh(self):
		return [self.Th[0][0],self.Th[1][0],self.Th[2][0]];
		
	def Rx(self):
		R1=[];
		R1.append([1,0,0]);
		R1.append([0,cos(self.thx),-sin(self.thx)]);
		R1.append([0,sin(self.thx),cos(self.thx)]);
		return numpy.array(R1);
		
	def Ry(self):
		R2=[];
		R2.append([cos(self.thy),0,-sin(self.thy)]);
		R2.append([0,1,0]);
		R2.append([sin(self.thy),0,cos(self.thy)]);
		return numpy.array(R2);
		
	def Rz(self):
		R3=[];
		R3.append([cos(self.thz),-sin(self.thz),0]);
		R3.append([sin(self.thz),cos(self.thz),0]);
		R3.append([0,0,1]);
		return numpy.array(R3);
	
	def R(self):
		#return self.Rz().dot(self.Ry().dot(self.Rx()));
		return self.Rx().dot(self.Ry().dot(self.Rz()));
		
		
	def tFrom(self,v):
		return self.R().dot(v)+self.c;
	
	def tTo(self,v):
		return numpy.linalg.inv(self.R()).dot(v-self.c);	

	def plotme(self,g,*args):
		scale=1.0;
		if(len(args)>0):
			scale=args[0];
		x=self.tFrom(numpy.array([[scale],[0],[0]]));
		y=self.tFrom(numpy.array([[0],[scale],[0]]));
		z=self.tFrom(numpy.array([[0],[0],[scale]]));
		o=self.tFrom(numpy.array([[0],[0],[0]]));
		g.plot([o[0][0],x[0][0]],[o[1][0],x[1][0]],[o[2][0],x[2][0]],"r");
		g.plot([o[0][0],y[0][0]],[o[1][0],y[1][0]],[o[2][0],y[2][0]],"g");
		g.plot([o[0][0],z[0][0]],[o[1][0],z[1][0]],[o[2][0],z[2][0]],"b");

# ====================================================		
# CAMERA
# ====================================================		
class Camera(CoordSys):
	focal_length=0.0;

	def __init__(self,c,Th):
		CoordSys.__init__(self,c,Th);
		#self.c=c;
		#self.Th=Th;
		
	def setFocalLength(self,focal_length):
		self.focal_length=focal_length;

	def orthographic(self,p):
		f=numpy.array([[0.0],[0.0],[-self.focal_length]]);
		nf2=f.transpose().dot(f);
		al=nf2/p.transpose().dot(f);
		al=al[0][0];
		ux=al*p[0][0];
		uy=al*p[1][0];
		return [ux,uy];
	
	def focalToCam(self,p):
		moons=Moons();
		preal=moons.focalToReal(p);
		pcam=self.tTo(preal);
		ucam=self.orthographic(pcam);
		return ucam;
	
	def camToReal(self,u):
		pcam=numpy.array([[u[0]],[u[1]],[-self.focal_length]]);
		preal=self.tFrom(pcam);
		return preal;
		
	def camToFocal(self,u):
		#Pc=numpy.array([[u[0]],[u[1]],[-self.focal_length]]);
		Pc=numpy.array([[u[0]],[u[1]],[-self.focal_length]]);
		print Pc;
		#print "---------------------"
		#print Pc;
		#print "---------------------"		
		rc=Moons().Rc;
		Rc=numpy.array([[-rc],[0.0],[0.0]]);
		Pw=self.tFrom(Pc);
		#print Pw;
		Cw=numpy.array([[self.c[0][0]],[self.c[1][0]],[self.c[2][0]]]);
		Dp=Pw-Cw;
		Dr=Rc-Cw;
		nDp2=innerp(Dp,Dp);
		nDr2=innerp(Dr,Dr);
		nDp=sqrt(nDp2);
		nDr=sqrt(nDr2);
		iprod=innerp(Dp,Dr);
		#print "########################"
		# --------------------------------------
		disc=iprod**2-(nDr2-rc**2)*nDp2;
		alp=(iprod+sqrt(disc))/nDp2;
		alm=(iprod-sqrt(disc))/nDp2;
		# --------------------------------------
		cosphi=iprod/nDp/nDr;
		alp=(nDr/nDp)*(cosphi+sqrt(cosphi**2-1+rc**2/nDr2));		
		alm=(nDr/nDp)*(cosphi-sqrt(cosphi**2-1+rc**2/nDr2));		
		# --------------------------------------
		if(alp<=0.0): al=alp;
		if(alm<=0.0): al=alm;
		#print al;
		#print "########################"		
		Xw=al*Pw+(1-al)*Cw;
		#print "=================="
		#print Xw;
		#print "=================="
		return Xw;

# ====================================================
# MOONS
# ====================================================		
class Moons:
	Rc=4.212;
	rdish=0.44;
	rcam=0.6;

	dh=Rc-Rc*sqrt(1.0-(rdish/Rc)**2);
	
	def getRealCoordSys(self):
		return CoordSys([0.0,0.0,0.0],[0.0,0.0,0.0]);
	
	def getCamera(self,th):
		h0=0.56;
		th1=53.5*pi/180.0;
		#th1=35.0*pi/180.0;
		c0=[-h0,self.rcam*cos(th),self.rcam*sin(th)];
		Th0=[pi/2+th,-th1,pi/2.0];
		focal_length=10e-3;
		camera=Camera(c0,Th0);
		camera.setFocalLength(focal_length);
		return camera;
	
	def plotDish(self,g):
		th=numpy.arange(0,2*pi,0.01);
		x=numpy.empty(len(th));
		x.fill(-self.dh);
		y=[self.rdish*cos(thi) for thi in th];
		z=[self.rdish*sin(thi) for thi in th];
		g.plot(x,y,z,"m");
		
	def focalToReal(self,p):
		rf=p[0];
		thf=p[1];
		x=-self.Rc*(1-cos(rf/self.Rc));
		y=self.Rc*sin(rf/self.Rc)*cos(thf);
		z=self.Rc*sin(rf/self.Rc)*sin(thf);
		return numpy.array([[x],[y],[z]]);
	
# ====================================================		
# DATA GENERATORS
# ====================================================		
class Data:
	def grid(self,sx,sy,nx,ny):
		data=[];
		for i in range(nx):
			for j in range(ny):
				data.append(numpy.array([[sx*i],[sy*j],[0]]));
		return data;
		
	def measurements1(self):
		table=[];
		table.append(numpy.array([[-251.0],[0.0],[0.0]]));
		table.append(numpy.array([[-249.0],[-75.0],[0.0]]));
		table.append(numpy.array([[-248.0],[121.0],[0.0]]));
		table.append(numpy.array([[0.0],[0.0],[0.0]]));
		table.append(numpy.array([[312.0],[-147.0],[0.0]]));
		table.append(numpy.array([[312.0],[197.0],[0.0]]));
		image=[];
		image.append(numpy.array([[-2032.0],[-101.0]]));
		image.append(numpy.array([[-2007.0],[-984.0]]));
		image.append(numpy.array([[-2005.0],[1382.0]]));
		image.append(numpy.array([[0.0],[0.0]]));
		image.append(numpy.array([[1514.0],[-1005.0]]));
		image.append(numpy.array([[1525.0],[1511.0]]));
		out=dict();
		out["table"]=table;
		out["image"]=image;
		return out;
		
# ====================================================		
# TRANSFORMATIONS
# ====================================================		
class Transf:
	# ====================================================
	# COORDINATE TRANSFORMATION FROM Cfrom to Cto
	# ====================================================
	def coordTransf(self,Cfrom,Cto,data):
		tmp=[Cfrom.tFrom(v) for v in data];
		out=[Cto.tTo(v) for v in tmp];
		return out;
			
	# ====================================================
	# ORTHOGRAPHIC TRANSFORMATION WITH FOCUS VECTOR v
	# GIVES A 3-D POINT ON THE FOCUS PLANE 
	# ====================================================
	def orthographic(self,f,data):
		nf2=f.transpose().dot(f);
		out=[nf2/x.transpose().dot(f)*x for x in data];
		return out;

	# ====================================================
	# RETURNS THE 2-D POINT ON THE FOCUS PLANE, AFTER
	# ORTHOGRAPHIC TRANSFORMATION
	# ====================================================
	def cameraPlane(self,f,data):
		xo=self.orthographic(f,data);
		if(f[1]==0.0 and f[2]==0.0):
			return [numpy.array([[v[1][0]],[v[2][0]]]) for v in xo];
		elif(f[0]==0.0 and f[2]==0.0):
			return [numpy.array([[v[0][0]],[v[2][0]]]) for v in xo];
		elif(f[0]==0.0 and f[1]==0.0):
			return [numpy.array([[v[0][0]],[v[1][0]]]) for v in xo];
		else:
			return [v-f for v in data];

	# ====================================================
	# RETURNS A 3-D POINT IN WORLD COORDINATE SYSTEM, FROM
	# A 2-D POINT ON THE FOCUS PLANE. ASSUMES POINTS LIE 
	# ON PLANE (x-n)'n=0
	# ====================================================
	def inverseCameraPlane(self,f,C,n,data):
		if(f[1]==0.0 and f[2]==0.0):
			pc=[numpy.array([[f[0][0]],[u[0][0]],[u[1][0]]]) for u in data];
		elif(f[0]==0.0 and f[2]==0.0):
			pc=[numpy.array([[u[0][0]],[f[1][0]],[u[1][0]]]) for u in data];
		elif(f[0]==0.0 and f[1]==0.0):
			pc=[numpy.array([[u[0][0]],[u[1][0]],[f[2][0]]]) for u in data];
		else:
			return None;
		#print "PC:"
		#print pc
		Cw=CoordSys([0.0,0.0,0.0],[0.0,0.0,0.0]);
		cw=C.c;
		pw=self.coordTransf(C,Cw,pc);
		#print "pw:";
		#print pw;
		#al=[(n-cw).transpose().dot(n)/(v-cw).transpose().dot(n) for v in pw];
		al=[(-cw).transpose().dot(n)/(v-cw).transpose().dot(n) for v in pw];
		#print al;
		xw=[al[i]*pw[i]+(1-al[i])*cw for i in range(len(pw))];
		return xw;
		
		
		
# ====================================================		
# PLOTTING
# ====================================================		
class Plot:
	def plot(self,g,data,marker):
		for v in data:
			if(v.shape[0]==2):
				g.plot([v[0][0]],[v[1][0]],marker);
			else:
				g.plot([v[0][0]],[v[1][0]],[v[2][0]],marker);
				
			
			
			
